#include "wii.h"
#include "player.h"
#include <stdio.h>
#include <iostream>
#include <time.h>
#include "wiimote.h"

struct wiiButtonsStruct {
    bool a, b, up, down, left, right, home, plus, minus, rumble;
}buttons;

struct wiiGesturesStruct {
    bool ccr, cr, du, dd;
}gestures;

struct wiiAxisStruct {
    int x,y,z;
}axis;

struct wiiStateStruct {
  wiiButtonsStruct buttons;
  wiiAxisStruct axis;
  wiiGesturesStruct gestures;
}wiiState;

wiimote_t wiimote = WIIMOTE_INIT;

Wii::Wii(){}

/**
  * initialize wiimote by connecting to its bluetooth MAC address and activate all leds to signalize connection
  *
  */

void Wii::wiiInit(){
    std::cout << "CONECTING TO WIIMOTE\n";
    //uni mote 00:19:1D:6C:C0:34
    //home mote 00:19:1D:D1:9B:D8
    //2nd home mote 00:19:1D:C7:B2:B1
    if(wiimote_connect(&wiimote, "00:19:1D:C7:B2:B1") != 0){
         std::cout << "CAN'T OPEN WIIMOTE\n";
    }
    else{
       std::cout << "WIIMOTE IS OPEN \n";
    }

    wiimote.led.one = 1;
    wiimote.led.two = 1;
    wiimote.led.three = 1;
    wiimote.led.four = 1;

    // enable accelerometer
    wiimote.mode.acc = 1;
}

/**
  * function running in seperate thread listening for buttons presses and accelerometers, updates the wiiState struct
  *
  */

void *Wii::wiiRun(void *arg){

    //start loop checking for keys
    while (wiimote_is_open(&wiimote)) {
        wiimote_update(&wiimote);

        // close connection if home is pressed and exit thread
        if (wiimote.keys.home) {
            wiimote_disconnect(&wiimote);
        }
        if(wiimote.keys.a){
            //printf("x=%d y=%d z=%d\n", wiimote.axis.x + normX, wiimote.axis.y + normY, wiimote.axis.z + normZ);
        }

        //update wii state in struct
        wiimote.keys.a == true ? wiiState.buttons.a = true : wiiState.buttons.a = false;
        wiimote.keys.b == true ? wiiState.buttons.b = true : wiiState.buttons.b = false;

        wiimote.keys.up == true ? wiiState.buttons.up = true : wiiState.buttons.up = false;
        wiimote.keys.down == true ? wiiState.buttons.down = true : wiiState.buttons.down = false;
        wiimote.keys.left == true ? wiiState.buttons.left = true : wiiState.buttons.left = false;
        wiimote.keys.right == true ? wiiState.buttons.right = true : wiiState.buttons.right = false;
        wiimote.keys.plus == true ? wiiState.buttons.plus = true : wiiState.buttons.plus = false;
        wiimote.keys.minus == true ? wiiState.buttons.minus = true : wiiState.buttons.minus = false;

        wiiState.axis.x = wiimote.axis.x;
        wiiState.axis.y = wiimote.axis.y;
        wiiState.axis.z = wiimote.axis.z;

        /*if(usleep(10000)){
            std::cout << "ERROR: Thread sleep unsuccesfull.\n";
        }*/
    }

    pthread_exit(NULL);
}

/**
  * returns bools whether a button is pressed or not that is passed as a parameter
  *
  */

bool Wii::isButtonDown(int button){

    switch(button){
    case UP:
        return wiiState.buttons.up;
        break;
    case DOWN:
        return wiiState.buttons.down;
        break;
    case LEFT:
        return wiiState.buttons.left;
        break;
    case RIGHT:
        return wiiState.buttons.right;
        break;
    case A:
        return wiiState.buttons.a;
        break;
    case B:
        return wiiState.buttons.b;
        break;
    case HOME:
        return wiiState.buttons.home;
        break;
    case PLUS:
        return wiiState.buttons.plus;
        break;
    case MINUS:
        return wiiState.buttons.minus;
        break;
    default:
        std::cout << "ERROR: invalid button specified.\n";
    }
}

/**
  * returns the axis value as int corresponding to the axis passed by parameter
  *
  */

int Wii::axisValue(int axis){
    switch(axis){
    case X:
        return wiiState.axis.x + normX;
        break;
    case Y:
        return wiiState.axis.y + normY;
        break;
    case Z:
        return wiiState.axis.z + normZ;
        break;
    default:
        std::cout << "ERROR: invalid axis specified.";
    }
}

/**
  * returns an Ogre vector containing the weighted accelerometer values
  *
  */

Ogre::Vector3 Wii::getVector(float time){

    int vectorWeight = 3000*time; //calculates the tracker vector according to fps time

    Ogre::Vector3 tVector = Ogre::Vector3(Wii::axisValue(X)*-vectorWeight, Wii::axisValue(Y)*vectorWeight, Wii::axisValue(Z)*-vectorWeight);
    return tVector;
}

bool Wii::getGesture(int gesture){

    switch(gesture){
    case CCR:
        return wiiState.gestures.ccr;
        break;
    case CR:
        return wiiState.gestures.cr;
        break;
    case DU:
        return wiiState.gestures.du;
        break;
    case DD:
        return wiiState.gestures.dd;
        break;
    default:
        std::cout << "ERROR: invalid gesture specified.";
    }
}

void Wii::setGesture(int gesture, bool recognised){

    switch(gesture){
    case CCR:
        wiiState.gestures.ccr = recognised;
        break;
    case CR:
        wiiState.gestures.cr = recognised;
        break;
    case DU:
        wiiState.gestures.du = recognised;
        break;
    case DD:
        wiiState.gestures.dd = recognised;
        break;
    default:
        std::cout << "ERROR: invalid gesture specified.";
    }
}

void Wii::wiiSetRumble(bool wiiRumble){
    wiiState.buttons.rumble = wiiRumble;
}

bool Wii::wiiGetRumble(){
    return wiiState.buttons.rumble;
}


void Wii::wiiRumble(bool wiiRumble){
    if(wiiRumble){
        wiimote.rumble = 1;
    }
    else{
        wiimote.rumble = 0;
    }
}

/**
  * custom modulo operator that supports negative numbers
  *
  */

int Wii::mod(int x, int m) {
    return (x%m + m)%m;
}
